//////////////////////////////////////////////////////////////////////////////
//    Copyright 2004, SenseGraphics AB
//
//    This file is part of H3D API.
//
//    H3D API is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    H3D API is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with H3D API; if not, write to the Free Software
//    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//    A commercial license is also available. Please contact us at 
//    www.sensegraphics.com for more information.
//
//
/// \file GeoDeformableShape.cpp
/// \brief CPP file for GeoDeformableShape.
///
//
//
//////////////////////////////////////////////////////////////////////////////

#include "GeoDeformableShape.h"
#include "GeoNeighbours.h"
#include "Coordinate.h"
#include "H3DHapticsDevice.h"
#include "TextureCoordinate.h"
#include "Appearance.h"
#include "ImageTexture.h"
#include "Normal.h"

//Mike Oren: These includes were added for various needs.
#include "FrictionalSurface.h"
#include "GeoMagneticSurface.h"
#include "Python.h"
#include "MultiTexture.h"
#include <math.h>
#include "ForceField.h"
#include "HapticForceEffect.h"
#include "X3D.h"
#include <iostream>
#include <fstream>

using namespace H3D;

//Mike Oren added this to figure out when to change the sound.
int lastSound = 0;
int lastHue = -1;
float curForce = 0;

int frameCounter = 0;
BYTE color[4];
BYTE sndColor[4];
ImageTexture *hapTex = NULL;
ImageTexture *sndTex = NULL;
int oldx = -9999,
    oldy = -9999;

// Add this node to the H3DNodeDatabase system.
H3DNodeDatabase GeoDeformableShape::database( "GeoDeformableShape", 
                                              &(newInstance<GeoDeformableShape>), 
                                              typeid( GeoDeformableShape ),
                                              &X3DShapeNode::database );

namespace GeoDeformableShapeInternals {
  FIELDDB_ELEMENT( GeoDeformableShape, origCoord, OUTPUT_ONLY );
  FIELDDB_ELEMENT( GeoDeformableShape, restingCoord, OUTPUT_ONLY );
  FIELDDB_ELEMENT( GeoDeformableShape, deformedCoord, OUTPUT_ONLY );
  FIELDDB_ELEMENT( GeoDeformableShape, deformer, INPUT_OUTPUT );
  FIELDDB_ELEMENT( GeoDeformableShape, shouldDeform, INPUT_OUTPUT );
  FIELDDB_ELEMENT( GeoDeformableShape, isGrasping, INPUT_OUTPUT );
  FIELDDB_ELEMENT( GeoDeformableShape, hapImg, INPUT_OUTPUT );
  FIELDDB_ELEMENT( GeoDeformableShape, sndImg, INPUT_OUTPUT );
  FIELDDB_ELEMENT( GeoDeformableShape, shouldDraw, INPUT_OUTPUT );
  FIELDDB_ELEMENT( GeoDeformableShape, velocity, INPUT_OUTPUT );
  FIELDDB_ELEMENT( GeoDeformableShape, key, INPUT_OUTPUT );
}


GeoDeformableShape::GeoDeformableShape(
                  Inst< SFAppearanceNode > _appearance,
                  Inst< SFGeometryNode   > _geometry,
                  Inst< SFHapticGeometry > _hapticGeometry,
                  Inst< SFNode           > _metadata,
                  Inst< SFBound          > _bound,
                  Inst< SFVec3f          > _bboxCenter,
                  Inst< SFVec3f          > _bboxSize,
                  Inst< SFCoordinateNode > _origCoord,
                  Inst< SFCoordinateNode > _restingCoord,
                  Inst< SFCoordinateNode > _deformedCoord,
                  Inst< SFCoordinateDeformer > _deformer,
				  Inst< SFBool > _shouldDeform,
				  Inst< SFBool > _isGrasping,
				  Inst< SFString > _hapImg,
				  Inst< SFString > _sndImg,
				  Inst< SFBool > _shouldDraw,
				  Inst< SFFloat > _velocity,
				  Inst< SFFloat > _key,
				  Inst< SFFloat > _hue,
				  Inst< SFFloat > _saturation,
				  Inst< SFFloat > _intensity):
  X3DShapeNode( _appearance, _geometry, _hapticGeometry, _metadata,
                   _bound, _bboxCenter, _bboxSize ),
  origCoord( _origCoord ),
  restingCoord( _restingCoord ), 
  deformedCoord( _deformedCoord ),
  deformer( _deformer ),
  shouldDeform(_shouldDeform),
  isGrasping(_isGrasping),
  hapImg(_hapImg),
  sndImg(_sndImg),
  shouldDraw(_shouldDraw),
  velocity(_velocity),
  key(_key),
  hue(_hue),
  saturation(_saturation),
  intensity(_intensity){

  type_name = "GeoDeformableShape";
  database.initFields( this );

  origCoord->setValue( new Coordinate, id );
  restingCoord->setValue( new Coordinate, id );
  deformedCoord->setValue( new Coordinate, id );
  isGrasping->setValue(0);
  shouldDraw->setValue(0);
  velocity->setValue(0);
  key->setValue(0);
  hue->setValue(0.0f);
  saturation->setValue(0.0f);
  intensity->setValue(0.0f);
}

  GeoDeformableShape::~GeoDeformableShape()
  {
  }

  //Mike Oren added this to convert RGB values to HSL values. This uses the formula found at http://en.wikipedia.org/wiki/HSL_color_space
void  RGBtoHSL(float r, float g, float b, float &h, float &s, float &l)
{
    float mn=r,mx=r;
    int maxVal=0;
 
    if (g > mx){ mx=g;maxVal=1;}
    if (b > mx){ mx=b;maxVal=2;} 
    if (g < mn) mn=g;
    if (b < mn) mn=b; 

    float  delta = mx - mn;
 
    l = (mx + mn)/2; 
    if( mx == 0 )
    {
      s = 0;
      h = 0;
      return;
    }
	else if(mn == mx)
		s = 0;
	else if(l > 0 && l < 0.5)
		s = (mx - mn)/(2 * l);
	else
		s = (mx - mn)/(2 * (2*l));


    if (s==0.0f)
    {
      h=0.0f;
      return;
    }
    else
    { 
      switch (maxVal)
      {
      case 0: if(g >= b) {h = 60 * (( g - b ) / delta);break;}
			  else {h = (60 * (( g - b ) / delta)) + 360;break;}
      case 1:{h = (60 * (( b - r ) / delta)) + 120;break;}     
      case 2:{h = (60 * (( r - g ) / delta)) + 240;break;}    
      }
    }
}

  //Mike Oren added this
std::string itos(int i)	// convert int to string
{
	stringstream s;
	s << i;
	return s.str();
}

void GeoDeformableShape::traverseSG( TraverseInfo &ti ) {
  X3DShapeNode::traverseSG( ti );

  X3DComposedGeometryNode *graphics_geom = dynamic_cast< X3DComposedGeometryNode * >( geometry->getValue() );
  if( !graphics_geom ) {
    if( geometry->getValue() )
      cerr << "Warning: Invalid geometry type in geometry field for "
           << "GeoDeformableShape node. Must be a X3DComposedGeometryNode." 
           << endl;
    return;
  }
  X3DGeometryNode *haptics_geom = hapticGeometry->getValue();
  if( !haptics_geom ) haptics_geom = graphics_geom;

  if( haptics_geom ) {
    Coordinate *coord_node = 
      dynamic_cast< Coordinate * >( graphics_geom->coord->getValue() );

    GeoH3DCoordinateDeformerNode *deformer_node = deformer->getValue();

    Coordinate *orig_coord = origCoord->getValue();
    Coordinate *resting_coord = restingCoord->getValue();
    Coordinate *deformed_coord = deformedCoord->getValue();

    if( deformer_node && coord_node ) {
		if(coord_node->point->size() != deformed_coord->point->size())
		{
			//Jon Sheller
			//Lines of magic to deal with the situation where the surface
			//changes the number of triangles
			deformed_coord->point->setValue(coord_node->point->getValue());
			resting_coord->point->setValue(coord_node->point->getValue());
			orig_coord->point->setValue(coord_node->point->getValue());
		}

      vector< Vec3f > new_resting_coords( resting_coord->point->size() );
      vector< Vec3f > new_deformed_coords( resting_coord->point->size() );

      vector< Vec3f > penetration_points;
      const vector< H3DHapticsDevice *> &haptics_devices = 
        ti.getHapticsDevices();
      for( vector< H3DHapticsDevice *>::const_iterator hd = 
             haptics_devices.begin();
           hd != haptics_devices.end(); hd++ ) {
        penetration_points.push_back( ti.getAccInverseMatrix() * 
                                      (*hd)->weightedProxyPosition->getValue() );
      }

      GeoH3DNeighboursNode* neighbours =
		  (static_cast<GeoDeformableShape::SFGeometryNode*>( geometry.get() ))->neighbourhood.get();
	  
	  H3DHapticsDevice* haptic = ti.getHapticsDevice(0);
	  X3DAppearanceNode *a = appearance->getValue();
	
	   //Jon Sheller
	   vector<DWORD> colors;
	   
	   //Mike Oren: Placed Jon's GetContactColor call in if statement so we only run it every 30 frames.
	   /*if (frameCounter==0)
	   {*/
		  
		   if(hapImg->getValue() != "" && isGrasping->getValue() == false){
				std::string tStr = hapImg->getValueAsString();
				//Mike Oren (Work in Progress): This code is meant to use a specific texture for sound or haptics, but using this method is currently slow. Fails if I make these globals.
				if (hapTex == NULL){
					hapTex = new ImageTexture;
				}
				if(hapTex->getURLUsed() != tStr){
					vector<std::string> urls;
					urls.push_back(tStr);
					hapTex->url->setValue(urls);
					hapTex->initialize();
				}
				if(GetContactColor(graphics_geom, colors, hapTex))
				{
					color[0] = (colors[0] & 0xff000000) >> 24;
					color[1] = (colors[0] & 0x00ff0000) >> 16;
					color[2] = (colors[0] & 0x0000ff00) >> 8;
					color[3] = (colors[0] & 0x000000ff);
				}
		   }
		   if(sndImg->getValue() != "" && isGrasping->getValue() == false){
			    std::string tStr = sndImg->getValueAsString();
				//Mike Oren (Work in Progress): This code is meant to use a specific texture for sound or haptics, but using this method is currently slow. Fails if I make these globals.
				if (sndTex == NULL){
					sndTex = new ImageTexture;
				}
				if(sndTex->getURLUsed() != tStr){
					vector<std::string> urls;
					urls.push_back(tStr);
					sndTex->url->setValue(urls);
					sndTex->initialize();
				}
				if(GetContactColor(graphics_geom, colors, sndTex))
				{
					sndColor[0] = (colors[0] & 0xff000000) >> 24;
					sndColor[1] = (colors[0] & 0x00ff0000) >> 16;
					sndColor[2] = (colors[0] & 0x0000ff00) >> 8;
					sndColor[3] = (colors[0] & 0x000000ff);
				}
		   }
		   if(haptic->mainButton->getValue() && shouldDraw->getValue())
				SetContactColor(graphics_geom, ti.getAccInverseMatrix() * haptic->force->getValue());
		   else{
			   oldx = -9999;
			   oldy = -9999;
		   }
		/*frameCounter++;
	   }*/
	  /*else{
		   frameCounter++;
		   if (frameCounter>=30)
               frameCounter = 0;
	   }*/

	   //Mike Oren: Convert RGB data to HSV for use.
	   float h, h2;
	   float s, s2, l, l2;
	   RGBtoHSL((float)color[1]/255.0,(float)color[2]/255.0,(float)color[3]/255.0, h, s, l);
	   RGBtoHSL((float)sndColor[1]/255.0,(float)sndColor[2]/255.0,(float)sndColor[3]/255.0, h2, s2, l2);
	   hue->setValue(h2);
	   saturation->setValue(s2);
	   intensity->setValue(l2);
		
	   if(haptic->mainButton->getValue() && isGrasping->getValue() == false && shouldDeform->getValue() && shouldDraw->getValue() == false){
		deformer_node->deformPoints( haptics_geom->isTouched->getValue(), 
                                   haptics_geom->contactPoint->getValue(),
                                   haptics_geom->contactNormal->getValue(),
                                   haptics_geom->force->getValue(),
                                   penetration_points,
								   *neighbours,
                                   orig_coord->point->getValue(),
                                   resting_coord->point->getValue(),
                                   deformed_coord->point->getValue(),
                                   new_resting_coords,
                                   new_deformed_coords );

		//Mike Oren: This code handles making the surface magnetic so that it may be easily pulled.
		if( a ) {
			GeoMagneticSurface* mag = dynamic_cast<GeoMagneticSurface*>(a->surface->getValue());
			if( mag ) {
				//change the stiffness value so that certain colors are harder to deform.
				mag->stiffness->setValue(abs((float)h-360.00)/360.00); //stiffness = inverse lightness
				mag->snapDistance->setValue(0.5f);
				mag->dynamicFriction->setValue(0.0f);
				mag->staticFriction->setValue(0.0f);
				mag->damping->setValue(0.2f);
			}
			appearance->setValue(a);
		}

		//Mike Oren: turn the sounds off when in deform mode.
		/*Py_Initialize();
		PyRun_SimpleString( "import MiDiSound as myMidi" );
		PyRun_SimpleString( "myMidi.stopSound()" );
		lastSound = 0;
		lastHue = -1;*/

		if( new_resting_coords.size() > 0 ) {
			resting_coord->point->setValue( new_resting_coords );
		}

		if( new_deformed_coords.size() > 0 ) {
			deformed_coord->point->setValue( new_deformed_coords );
			coord_node->point->setValue( new_deformed_coords );
		}
	  }

	  //Mike Oren added this line, it sets the haptic geometry so that it matches the graphics geometry
	  hapticGeometry->setValue(graphics_geom, id);
	  //Mike Oren: resetthe surface to be non-magnetic and set the friction according to color touched
	  //Mike Oren: This short script will play a sound!
	  if((graphics_geom->contactPoint->getValue().size() > 0) && (graphics_geom->isTouched->getValue().size() > 0) && (graphics_geom->isTouched->getValue()[0])){
			if (sndImg->getValue() != ""){
				key->setValue((0.3 * sndColor[1]) + (0.59 * sndColor[2]) + (0.11 * sndColor[3])); //convert RGB to Grey
			}
			else{
				Vec3f mMatrix = ti.getAccInverseMatrix() * haptic->proxyPosition->getValue();
				key->setValue(mMatrix[2] * 1000 + 15);
				if (key->getValue() > 255)
					key->setValue(255);
				if (key->getValue() < 0)
					key->setValue(0);
			}
			velocity->setValue((abs(haptic->force->getValue()[0]) + abs(haptic->force->getValue()[1]) + abs(haptic->force->getValue()[2])) / 3);
			if (key->getValue() != lastSound || curForce != velocity->getValue()){
				curForce = velocity->getValue();
				/*Py_Initialize();
				PyRun_SimpleString( "import MiDiSound as myMidi" );
				std::string part1 = "myMidi.playSound(0,";*/
				//int key = haptic->devicePosition->getValue()[1];
				/*if (key < -65 && key >= -75) key = 30;
				else if (key < -55)	key = 35;
				else if (key < -45)	key = 40;
				else if (key < -35)	key = 45;
				else if (key < -25)	key = 50;
				else if (key < -15)	key = 55;
				else if (key < -5)	key = 60;
				else if (key < 15)	key = 65;
				else if (key < 25)	key = 70;
				else if (key < 35)	key = 75;
				else if (key < 45)	key = 80;
				else if (key < 55)	key = 85;
				else if (key < 65)	key = 90;
				else if (key <= 75)	key = 95;
				else{ cout<<key<<endl; key = 115;}*/
				/*cout<<"RGB: "<<(int)color[1]<<" "<<(int)color[2]<<" "<<(int)color[3]<<endl;
				cout<<h<<" "<<s<<" "<<l<<endl;*/
				int instrument = 20; //h / 30;
				//instrument *= 10; //cut the right most number and replace it with a 0, this eliminates minor changes.
				//Vec3f mMatrix = ti.getAccForwardMatrix().getRotationPart() * haptic->proxyPosition->getValue();
				//int key = mMatrix[2] * 1000 + 15;//l * 10;
				/*if (key < 1) key = 25;
				else if (key < 2) key = 35;
				else if (key < 3) key = 45;
				else if (key < 4) key = 55;
				else if (key < 5) key = 65;
				else if (key < 6) key = 75;
				else if (key < 7) key = 85;
				else if (key < 8) key = 95;
				else if (key < 9) key = 105;
				else key = 115;*/

				if(velocity->getValue() < 1)
					velocity->setValue(25);
				else if(velocity->getValue() < 2)
					velocity->setValue(45);
				else if(velocity->getValue() < 3)
					velocity->setValue(65);
				else if(velocity->getValue() < 4)
					velocity->setValue(85);
				else if(velocity->getValue() < 5)
					velocity->setValue(105);
				else
					velocity->setValue(127);
				std::string part2 = ",";
				std::string part3 =	",";
				std::string part4 = ")";
				//std::string execution = part1 + itos(key->getValue()) + part2 + itos(instrument) + part3 + itos((int)velocity->getValue()) + part4;
				/*PyRun_SimpleString( execution.c_str() );*/
				lastHue = h / 30;
				lastSound = key->getValue();
			}
			//else{
			//	lastSound++;
			//	//if (h / 30 != lastHue){
			//	if(lastSound > 100){
			//		lastSound = 0;
			//	}
			//}
	  }
		else{
			/*Py_Initialize();
			PyRun_SimpleString( "import MiDiSound as myMidi" );
			PyRun_SimpleString( "myMidi.stopSound()" );*/
			key->setValue(0);
			velocity->setValue(0);
			lastSound = 0;
			lastHue = -1;
			curForce = 0;
		}
	  if(!haptic->mainButton->getValue() || shouldDraw->getValue()){
		if( a ) {
			GeoMagneticSurface* mag = dynamic_cast<GeoMagneticSurface*>(a->surface->getValue());
			if( mag ) {
				mag->snapDistance->setValue(-1.0f); //must be negative otherwise it will still be magnetic
				mag->stiffness->setValue(0.7f);
				mag->damping->setValue(0.7f);
				if(graphics_geom->contactPoint->getValue().size() > 0){
					mag->dynamicFriction->setValue(l);
					mag->staticFriction->setValue(l);
					if (h > 235 && h < 245){
						//ForceField force;
						mag->dynamicFriction->setValue(0.0f);
						mag->staticFriction->setValue(0.0f);
						//force.force->setValue(Vec3f(0.7,0,0));
						//ti.addForceEffectToAll(new HapticForceField( ti.getAccForwardMatrix(), force.force->getValue(), true));
					}
				}					
				else{
					mag->dynamicFriction->setValue(0.0f);
				    mag->staticFriction->setValue(0.0f);
				}
			}
			appearance->setValue(a);
		}
	  }
	  else if(isGrasping->getValue()){
		  if( a ) {
			GeoMagneticSurface* mag = dynamic_cast<GeoMagneticSurface*>(a->surface->getValue());
			if( mag ) {
				mag->snapDistance->setValue(-1.0f); //must be negative otherwise it will still be magnetic
				mag->stiffness->setValue(0.0f);
				mag->damping->setValue(0.0f);
				mag->dynamicFriction->setValue(0);
				mag->staticFriction->setValue(0);			
			}
			appearance->setValue(a);
		}
	  }
    }
  }
}

// Jon Sheller
// This fills the colors into a vector of DWORDs. This is a bit hackish, but it should
// work perfectly fine.
// TODO: Check on color data types and possibly convert to use those for the sake of readability
bool GeoDeformableShape::GetContactColor(X3DComposedGeometryNode *geom, vector<DWORD> & colors, ImageTexture *tex)
{
	if(!(geom->isTouched->getValue().size() > 0))
		return false;

	if(!geom->isTouched->getValue()[0])
		return false;

	if(!geom->arraysSet)
	{
		cout<<"No arrays."<<endl;
		return false;
	}

	if(!(geom->contactPoint->getValue().size() > 0))
	{
		cout<<"No contact point."<<endl;
		return false;
	}

	Vec3f pt = geom->contactPoint->getValue()[0];
	  float x = geom->sparams[0] * pt[0] + geom->sparams[1] * pt[1] + geom->sparams[2] * pt[2] + 0.5;
	  float y = geom->tparams[0] * pt[0] + geom->tparams[1] * pt[1] + geom->tparams[2] * pt[2] + 0.5;

	  if(x < 0.0)
		  x = 0.0;

	  if(y < 0.0)
		  y = 0.0;

	  if(x > 1.0)
		  x = 1.0;

	  if(y > 1.0)
		  y = 1.0;

	  Appearance *app;
	  Image *image;
	  MultiTexture *mTex;
	  vector<ImageTexture*> textures;

	  app = dynamic_cast<Appearance*>(appearance->getCastedValue());
	  if(app == NULL)
	  {
		  cout<<"No appearance"<<endl;
		  return false;
	  }

	  // Jon Sheller TODO: Change this to use caching--there's no point in doing all these
	  // calculations every frame
	  if(tex == NULL)
	  {
		  cout<<"No image texture"<<endl;
		  cout<<"Trying multitexture"<<endl;
		  mTex = dynamic_cast<MultiTexture*>(app->texture->getValue());
		  if(mTex == NULL)
		  {
		    //cout<<"MultiTexture not found."<<endl;
			return false;
		  }
		  else
		  {
			  for(int i=0; i<mTex->texture->getValue().size(); i++)
			  {
				  ImageTexture * tImg = dynamic_cast<ImageTexture*>(mTex->texture->getValue()[i]);
				  if(tImg != NULL)
				  {
					  textures.push_back(tImg);
				  }
			  }
		  }
	  }
	  else
	  {
		  textures.push_back(tex);
	  }

	  bool foundColor = false;
	  DWORD color;
	  for(int i=0; i < textures.size(); i++)
	  {
		image = dynamic_cast<Image*>(textures[i]->image->getValue());
		if(image == NULL)
			continue;
		
		foundColor = true;

		// TODO Jon Sheller
		// The lines below truncate the result of the multiplication, when it might be more
		// correct to do bilinear averaging or something of the sort
		int xC = int(x * image->width());
		int yC = int(y * image->height());

		//cout<<"Image: ("<<xC<<", "<<yC<<")"<<endl;
		image->getElement((void *)(&color), xC, yC);
		colors.push_back(color);
	  }

	return foundColor;
}

//Mike Oren: Draw color.
void GeoDeformableShape::SetContactColor(X3DComposedGeometryNode *geom, Vec3f thicknessVec)
{
	if(!(geom->isTouched->getValue().size() > 0))
		return;

	if(!geom->isTouched->getValue()[0]){
		oldx = -9999;
		oldy = -9999;
		return;
	}

	if(!geom->arraysSet)
	{
		cout<<"No arrays."<<endl;
		return;
	}

	if(!(geom->contactPoint->getValue().size() > 0))
	{
		cout<<"No contact point."<<endl;
		oldx = -9999;
		oldy = -9999;
		return;
	}

	Vec3f pt = geom->contactPoint->getValue()[0];
	  float x = geom->sparams[0] * pt[0] + geom->sparams[1] * pt[1] + geom->sparams[2] * pt[2] + 0.5;
	  float y = geom->tparams[0] * pt[0] + geom->tparams[1] * pt[1] + geom->tparams[2] * pt[2] + 0.5;
	  float z = geom->rparams[0] * pt[0] + geom->rparams[1] * pt[1] + geom->rparams[2] * pt[2] + 0.5;

	  if(x < 0.0)
		  x = 0.0;

	  if(y < 0.0)
		  y = 0.0;

	  if(z < 0.0)
		  z = 0.0;

	  if(x > 1.0)
		  x = 1.0;

	  if(y > 1.0)
		  y = 1.0;

	  if(z > 1.0)
		  z = 1.0;

	  Appearance *app;
	  ImageTexture *tex;
	  std::vector<ImageTexture*> textures;
	  Image *image;
	  MultiTexture *mTex;

	  app = dynamic_cast<Appearance*>(appearance->getCastedValue());
	  if(app == NULL)
	  {
		  cout<<"No appearance"<<endl;
		  return;
	  }

	 tex = dynamic_cast<ImageTexture*>(app->texture->getValue());

	  if(tex == NULL)
	  {
		  cout<<"No image texture"<<endl;
		  cout<<"Trying multitexture"<<endl;
		  mTex = dynamic_cast<MultiTexture*>(app->texture->getValue());
		  if(mTex == NULL)
		  {
		    //cout<<"MultiTexture not found."<<endl;
			return;
		  }
		  else
		  {
			  for(int i=0; i<mTex->texture->getValue().size(); i++)
			  {
				  ImageTexture * tImg = dynamic_cast<ImageTexture*>(mTex->texture->getValue()[i]);
				  if(tImg != NULL)
				  {
					  textures.push_back(tImg);
				  }
			  }
		  }
	  }
	  else
	  {
		  textures.push_back(tex);
	  }

	  DWORD color;
	  for(int i=0; i < textures.size(); i++)
	  {
		image = dynamic_cast<Image*>(textures[i]->image->getValue());
		if(image == NULL)
			continue;
		// TODO Jon Sheller
		// The lines below truncate the result of the multiplication, when it might be more
		// correct to do bilinear averaging or something of the sort
		int xC = int(x * image->width());
		int yC = int(y * image->height());

		color = 0x00ff0000;

		//Mike Oren: Line drawing.
		if(oldx != -9999 && oldy != -9999){
			int x1, x2, dx, dy, y1, y2;
			x1 = xC;
			x2 = oldx;
			y1  = yC;
			y2  = oldy;

			int thickness = abs(thicknessVec[2]);
			if (thickness <= 0)
				thickness = 1;

			int x=x1,y=y1,s1,s2;
			int length,i,dp,temp,swap=0;

			for(int k=0; k<thickness; k++)
			   for (int l=0; l<thickness; l++)
				image->setElement((void *)(&color), x1+k, y1+l);
			dx=abs(x2-x1);
			dy=abs(y2-y1);

			if(x2<x1) s1=-1;
			else if(x2>x1) s1=1;
			else s1=0;

			if(y2<y1) s2=-1;
			else if(y2>y1) s2=1;
			else s2=0;

			dp=2*dy-dx;
			if(dy>dx)
			{
				temp=dx;
				dx=dy;
				dy=temp;
				swap=1;
			}
			for(i=1;i<=dx;i++)
			{
				cout<<i<<endl;
				if(dp<0)
				{
					if(swap){
						 y=y+s2;
						 for(int k=0; k<thickness; k++)
						   for (int l=0; l<thickness; l++)
							image->setElement((void *)(&color), x+k, y+l);
					}
					else{
						 x=x+s1;
						 for(int k=0; k<thickness; k++)
						   for (int l=0; l<thickness; l++)
							image->setElement((void *)(&color), x+k, y+l);
					}
					 dp+=2*dy;
				}
				else
				{
					x=x+s1;
					y=y+s2;
					for(int k=0; k<thickness; k++)
					   for (int l=0; l<thickness; l++)
						image->setElement((void *)(&color), (x)+k, (y)+l);
					dp=dp+2*dy-2*dx;
				}
			}
		}
		oldx = xC;
		oldy = yC;

		tex->image->setValue(image);
		app->texture->setValue(tex);
	  }
}

//Mike Oren: Everything below here was added with the ChainmailDeformer Add-On

// First step of neighbours calculation according to the mesh of the shape.
vector< vector< unsigned long > >
GeoDeformableShape::SFGeometryNode::coordinateIndexPerFace( IndexedFaceSet* n )
{
	vector< vector< unsigned long > > res;
	vector< unsigned long > face;
	vector< int > coord_index( n->coordIndex->getValue() );

	// Create faces knowing that an index of "-1" indicates that the current face has ended and the next one begins.
	for( unsigned int i (0); i < coord_index.size(); i++ ) {
		face.clear();
		while( i < coord_index.size() && coord_index[i] != -1 )    
			face.push_back( coord_index[i++] );
		res.push_back( face );
	}

	return res;
}

vector< vector< unsigned long > >
GeoDeformableShape::SFGeometryNode::coordinateIndexPerFace( IndexedTriangleFanSet* n )
{
	vector< vector< unsigned long > > res;
	vector< unsigned long > face (3);
	// the number of triangles rendered so far in this triangle strip
	unsigned int fan_root;
	vector< int > indices( n->index->getValue() );

    for( unsigned int i = 0; i < indices.size() - 2; i++ ) {
		for( fan_root = i; i < indices.size() - 2 && indices[i] != -1; i++ ) {
          if( indices[i+1] != -1 && indices[i+2] != -1 ) {
			face[0] = indices[fan_root];
			face[1] = indices[i+1];
			face[2] = indices[i+2];
			res.push_back( face );
          }
        }
	}

/*	result never checked
	cout << "RES" << endl;
	for( vector< vector<unsigned long> >::iterator titi(res.begin()); titi!=res.end(); titi++ )
	{	cout << "[ ";
		for ( vector<unsigned long>::iterator toto(titi->begin()); toto!=titi->end(); toto++ )
			cout << *toto << " ";
		cout << " ] |";
	}
	cout << endl;
*/	return res;
}

vector< vector< unsigned long > >
GeoDeformableShape::SFGeometryNode::coordinateIndexPerFace( IndexedTriangleSet* n )
{	vector< vector< unsigned long > > res;
	vector< unsigned long > face (3);
	vector< int > index( n->index->getValue() );

	for( unsigned int j = 0; j+2 < index.size(); j+=3 ) {
		face[ 0 ] = index[ j ];
		face[ 1 ] = index[ j+1 ];
		face[ 2 ] = index[ j+2 ];
		res.push_back( face );
	}

	return res;
}

vector< vector< unsigned long > >
GeoDeformableShape::SFGeometryNode::coordinateIndexPerFace( IndexedTriangleStripSet* n )
{	vector< vector< unsigned long > > res;
	vector< unsigned long > face (3);
	vector< int > indices( n->index->getValue() );
	
	// neighbourhood dosen't pay attention for face orientation
	// consequently this code is "easier"
	for( unsigned int i = 0; i < indices.size() - 2; i++ ) {
		for( ; i < indices.size() - 2 && indices[i] != -1; i++ ) {
			if( indices[i+1] != -1 && indices[i+2] != -1 ) {
				face[0] = indices[i];
				face[1] = indices[i+1];
				face[2] = indices[i+2];
				res.push_back( face );
			}
		}
	}

/*	result never checked
	cout << "RES" << endl;
	for( vector< vector<unsigned long> >::iterator titi(res.begin()); titi!=res.end(); titi++ )
	{	cout << "[ ";
		for ( vector<unsigned long>::iterator toto(titi->begin()); toto!=titi->end(); toto++ )
			cout << *toto << " ";
		cout << " ] |";
	}
	cout << endl;
*/	return res;
}

vector< vector< unsigned long > >
GeoDeformableShape::SFGeometryNode::coordinateIndexPerFace( TriangleFanSet* n )
{	vector< vector< unsigned long > > res;
	vector< unsigned long > face (3);
	const vector< int > fan_count( n->fanCount->getValue() );

	  // the current vertex index
      unsigned int vertex_counter = 0;

      // the index of the root vertex of the current fan
      unsigned int fan_root =0;

      for( vector<int>::const_iterator sc = fan_count.begin();
           sc != fan_count.end();
           sc++ ) {      

        // check that fan count value >=3
        if( (*sc) < 3 ) {
          stringstream s;
          s << "Must be >=3 in \"" 
            << getName() << "\" node. ";
		  throw TriangleFanSet::InvalidFanCount( *sc, s.str(), H3D_FULL_LOCATION );
        }

        for( int i = 0; i < (*sc) - 2; i++ ) {
		  face[0] = fan_root;
		  face[1] = vertex_counter+1;
          face[2] = vertex_counter+2;
		  res.push_back( face );
          vertex_counter++;
        }
        
		// if( (*sc) >=3 )
		// skip to the next triangle fan
		vertex_counter += 2;

        fan_root += *sc;
      }

/*	never checked
	cout << "RES" << endl;
	for( vector< vector<unsigned long> >::iterator titi(res.begin()); titi!=res.end(); titi++ )
	{	cout << "[ ";
		for ( vector<unsigned long>::iterator toto(titi->begin()); toto!=titi->end(); toto++ )
			cout << *toto << " ";
		cout << " ] |";
	}
	cout << endl;
*/	return res;
}

vector< vector< unsigned long > >
GeoDeformableShape::SFGeometryNode::coordinateIndexPerFace( TriangleSet* n )
{	vector< vector< unsigned long > > res;
	vector< unsigned long > face (3);
	unsigned long	i(0),
					nr_coord( n->coord->getValue()->nrAvailableCoords() );

	while( i+2 < nr_coord )
	{	face[ 0 ] = i;
		face[ 1 ] = i+1;
		face[ 2 ] = i+2;
		res.push_back( face );
		i+=3;
	}

/*	never checked
	cout << "RES" << endl;
	for( vector< vector<unsigned long> >::iterator titi(res.begin()); titi!=res.end(); titi++ )
	{	cout << "[ ";
		for ( vector<unsigned long>::iterator toto(titi->begin()); toto!=titi->end(); toto++ )
			cout << *toto << " ";
		cout << " ] |";
	}
	cout << endl;
*/	return res;
}

vector< vector< unsigned long > >
GeoDeformableShape::SFGeometryNode::coordinateIndexPerFace( TriangleStripSet* n )
{	vector< vector< unsigned long > > res;
	vector< unsigned long > face (3);
	vector< int> strip_count( n->stripCount->getValue() );
	unsigned int vertex_count = 0;

	for( vector<int>::const_iterator sc = strip_count.begin();
	  sc != strip_count.end();
	  sc++ ) {
		for( int j = 0; j < (*sc) - 2; j++ ) {
			face[ 0 ] = vertex_count;
			face[ 1 ] = vertex_count+1;
			face[ 2 ] = vertex_count+2;
			res.push_back( face );
			vertex_count++;
		}
		if( (*sc) >=3 )
			// skip to the next triangle strip
			vertex_count += 2;
	}

/*	never checked
	cout << "RES" << endl;
	for( vector< vector<unsigned long> >::iterator titi(res.begin()); titi!=res.end(); titi++ )
	{	cout << "[ ";
		for ( vector<unsigned long>::iterator toto(titi->begin()); toto!=titi->end(); toto++ )
			cout << *toto << " ";
		cout << " ] |";
	}
	cout << endl;
*/	return res;
}

// second step of neighbours computation
GeoH3DNeighboursNode *GeoDeformableShape::SFGeometryNode::generateNeighbours
			( const vector< vector< unsigned long > > &indexPerFace )
{
	unsigned int neighbourhoodIndex(0);
	vector< vector< unsigned long > >::const_iterator	face( indexPerFace.begin() ),
								endFace( indexPerFace.end() );
	vector< unsigned long >::const_iterator	firstIndex,
						index,
						lastIndex,
						endIndex;
	map< unsigned int, vector< unsigned int > >	neighboursPerIndex;
	map< unsigned int, vector< unsigned int > >::iterator
					neighbours,
					endNeighbours( neighboursPerIndex.end() );
	vector< H3DInt32 >	res;
	GeoNeighbours*		ret = new GeoNeighbours;

	// for each coordinate of each face,
	// the others coordinates of the face are considered like neighbours.
	while( face != endFace )
	{
		firstIndex = face->begin();
		index = face->begin()+1;
		lastIndex = face->end()-1;
		endIndex = face->end();

		// check if the index is already referenced.
		neighbours=neighboursPerIndex.find( *firstIndex );
		if ( neighbours != endNeighbours )
			neighbours->second.insert( neighbours->second.begin(), index, endIndex );
		else
			neighboursPerIndex[ *firstIndex ] = vector< unsigned int >( index, endIndex );
		while ( index != lastIndex )
		{	// check if the index is already referenced.
			neighbours=neighboursPerIndex.find( *index );
			if ( neighbours != endNeighbours )
			{	neighbours->second.insert( neighbours->second.begin(), firstIndex, index );
				neighbours->second.insert( neighbours->second.begin(), index+1, endIndex );
			}
			else
			{	vector< unsigned int > tmp( index, endIndex );
				tmp.insert( tmp.begin(), index+1, endIndex );
				neighboursPerIndex[ *index ] = tmp;
			}
			index++;
		}
		// check if the index is already referenced.
		neighbours=neighboursPerIndex.find( *lastIndex );
		if ( neighbours != endNeighbours )
			neighbours->second.insert( neighbours->second.begin(), firstIndex, endIndex );
		else
			neighboursPerIndex[ *lastIndex ] = vector< unsigned int >( firstIndex, endIndex );

		face++;
	}

	neighbours = neighboursPerIndex.begin();
	// sort neighbourhood according to its index
	// sort( neighbours, endNeighbours ); useless
	while( neighbours != endNeighbours )
	{
		while( neighbours->first < neighbourhoodIndex )
		{	res.push_back( -1 );
			neighbourhoodIndex++;
		}
		sort( neighbours->second.begin(), neighbours->second.end() );
		res.insert( res.end(), neighbours->second.begin(),
		  unique( neighbours->second.begin(), neighbours->second.end() ) );
		res.push_back( -1 );
		neighbourhoodIndex++;
		neighbours++;
	}

	ret->vector->setValue( res );
	return ret;
}

void GeoDeformableShape::SFGeometryNode::onAdd( Node *n ) {
        X3DComposedGeometryNode *cg = 
          dynamic_cast< X3DComposedGeometryNode * >( n );
        if( !cg ) {
          stringstream s;
          s << "Expecting X3DComposedGeometryNode";
          throw InvalidNodeType( n->getTypeName(),
                                 s.str(),
                                 H3D_FULL_LOCATION );
        }
        // uphold a route between the encapsulated X3DGeometryNode's bound field to the bound field of the X3DShapeNode the field resides in.
        X3DShapeNode::SFGeometryNode::onAdd( n );
		// update coordinate properties
        GeoDeformableShape *dg = 
          static_cast< GeoDeformableShape * >( getOwner() );
        if( n ) {
          Coordinate *cg_coord = 
            dynamic_cast< Coordinate * >( cg->coord->getValue() ); 
          Coordinate *dg_orig_coord = dg->origCoord->getValue(); 
          Coordinate *dg_deformed_coord = dg->deformedCoord->getValue(); 
          Coordinate *dg_resting_coord = dg->restingCoord->getValue(); 
          if( cg_coord ) {
            if( dg_orig_coord ) {
              dg_orig_coord->point->setValue(
						cg_coord->point->getValue() );
            }
            if( dg_deformed_coord ) {
              dg_deformed_coord->point->setValue(
						cg_coord->point->getValue() );
            }
            if( dg_resting_coord ) {
              dg_resting_coord->point->setValue(
						cg_coord->point->getValue() );
            }
          }
        }

		// update neighbourhood properties
		if ( dynamic_cast<IndexedFaceSet*>( cg ) ) {
			neighbourhood.reset( generateNeighbours( coordinateIndexPerFace(
				static_cast<IndexedFaceSet*>( cg ) ) ) );
		}
		else if ( dynamic_cast<IndexedTriangleFanSet*>( cg ) ) {
			neighbourhood.reset( generateNeighbours( coordinateIndexPerFace(
				static_cast<IndexedTriangleFanSet*>( cg ) ) ) );
		}
		else if ( dynamic_cast<IndexedTriangleSet*>( cg ) ) {
			neighbourhood.reset( generateNeighbours( coordinateIndexPerFace(
				static_cast<IndexedTriangleSet*>( cg ) ) ) );
		}
		else if ( dynamic_cast<IndexedTriangleStripSet*>( cg ) ) {
			neighbourhood.reset( generateNeighbours( coordinateIndexPerFace(
				static_cast<IndexedTriangleStripSet*>( cg ) ) ) );
		}
		else if ( dynamic_cast<TriangleFanSet*>( cg ) ) {
			neighbourhood.reset( generateNeighbours( coordinateIndexPerFace(
				static_cast<TriangleFanSet*>( cg ) ) ) );
		}
		else if ( dynamic_cast<TriangleSet*>( cg ) ) {
			neighbourhood.reset( generateNeighbours( coordinateIndexPerFace(
				static_cast<TriangleSet*>( cg ) ) ) );
		}
		else if ( dynamic_cast<TriangleStripSet*>( cg ) ) {
			neighbourhood.reset( generateNeighbours( coordinateIndexPerFace(
				static_cast<TriangleStripSet*>( cg ) ) ) );
		}
      }
